#include "WallAvoidanceBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "BPhysicsMotionState.h"
#include "WanderBehaviour.h"
#include "SeekBehaviour.h"
#include "AvoidanceBehaviour.h"
#include "Engine.h"
#include "LineDrawer.h"

// Real Time Collision Detection: Christer Ericson
float WallAvoidanceBehaviour::Get2DTriangleArea(const btVector3& pA, const btVector3& pB, const btVector3& pC)
{
    return (pA.x() - pC.x()) * (pB.z() - pC.z()) - (pA.z() - pC.z()) * (pB.x() - pC.x());
}
bool WallAvoidanceBehaviour::LineSegTest(const btVector3& pA, const btVector3& pB,  //line1
                                         const btVector3& pC, const btVector3& pD,  //line2
                                         btVector3& p)                    //params 
{
    //Get the sign of the areas coresponding to the triangles made
    //by each line
    float area1 = Get2DTriangleArea(pA, pB, pD);
    float area2 = Get2DTriangleArea(pA, pB, pC);
    //these must have opposite signs to intersect
    if(area1 * area2 < 0.0f)
    {
        //compute signs for A to B in respect to segment CD
        float area3 = Get2DTriangleArea(pC, pD, pA);
        //we could compute this
        //float area4 = Get2DTriangleArea(pC, pD, pB);
        //but it is cheaper and faster to just do this...
        float area4 = area3 + area2 - area1;

        //now check if points a And b are on the same side
        if(area3 * area4 < 0.0f)
        {
            //we are here, and A and B are on different sides of line C-D
            //the equation is: L(t) = a + t * (b - a)
            float t = area3 / (area3 - area4);
            p = pA + t * (pB - pA);
            return true;
        }
    }
    return false;
}
btVector3 WallAvoidanceBehaviour::CalculateNormalWall( int index, const btTransform& m )
{
    //this only works if the walls are created in a certain order
    /*  __1__
    *   |   |
    * 4 |   | 2
    *   |   |
    *   |   |
    *   -----
          3
    */
    btTransform matrix(m);
    matrix.setOrigin(btVector3(0,0,0));
    btVector3 normal(1,0,0);
    switch(index)
    {
    case 0:
        {
            //this is the top,
            normal.setValue(0,0,1);
            break;
        }
    case 2:
        {
            //this is the right,
            normal.setValue(0,0,1);
            break;
        }
    case 4:
        {
            //this is the bottom,
            normal.setValue(0,0,-1);
            break;
        }
    case 6:
        {
            //this is the right,
            normal.setValue(-1,0,0);
            break;
        }
    }
    return (matrix * normal);
}
void WallAvoidanceBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    //check validity
    if(!info->GetSourceActor())
        return;

    btVector3 probes[3];
    //create the first probe
    btVector3 x = info->GetSourceActor()->GetHeading();
    x.setY(0);
    if(x.length2() < 0.001f)
        x.setValue(0,0,1);
    x.normalize();
    probes[0] = x;
    //create the second probe (to the right)
    btVector3 y(0,1,0);
    btVector3 z = y.cross(probes[0]);
    z.setY(0);
    z.normalize();
    probes[1] = probes[0] + z;
    probes[1].normalize();
    //create the third probe
    probes[2] = probes[0] - z;
    probes[2].normalize();
    for(int ml = 0; ml < 3; ++ml)
        probes[ml] *= info->GetWallAvoidDistance();
    //Ok so we have our lines ( position to probe[i] )
    //we now check each box's set of 4 lines
    vActors& actors = *info->GetActorVector();
    WallAvoidCache cache;
    for(int i = 0; i < actors.size(); ++i)
    {
        BPhysicsActor* a = actors[i];
        //if  we are not some sort of sphere or plane or other box
        if(a->IsStaticObstacle() && a->GetAiCollisionRadius() < 0)
        {
            //calculate the 4 lines that make up this shape
            btTransform trans = a->GetBodyPointer()->getWorldTransform();
            btVector3 scale = a->GetMotionState().GetGraphicsScale();
            scale;
            btVector3 points[8];
            memset(points, 0, sizeof(btVector3)*8);
            //line1
            points[0].setValue(scale.x(), 0 , scale.z());
            points[1].setValue(-scale.x(), 0 , scale.z());
            //line2
            points[2].setValue(scale.x(), 0 , scale.z());
            points[3].setValue(scale.x(), 0 , -scale.z());
            //line3
            points[4].setValue(scale.x(), 0 , -scale.z());
            points[5].setValue(-scale.x(), 0 , -scale.z());
            //line4
            points[6].setValue(-scale.x(), 0 , -scale.z());
            points[7].setValue(-scale.x(), 0 , scale.z());
            //now take the lines in local space into world space
            for(int tr = 0; tr < 8; ++tr)
            {
                points[tr] = trans*points[tr];
                points[tr].setY(0);
            }
            //these are used in the test
            btVector3 pA = info->GetSourceActor()->GetPosition();
            pA.setY(0);
            btVector3 pos(0,0,0);
            //now, for each probe
            for(int p = 0; p < 3; ++p)
            {
                for( int line = 0; line < 8; ++line)//increments twice in loop, for 4 lines
                {
                    int lp1 = line +1;//line pluss one
                    if(LineSegTest(pA, pA + probes[p],points[line],points[lp1],pos))//if they intersect
                    {
                        float dist = (pA - pos).length2();
                        if( dist < cache.smallestDistance)
                        {
                            cache.bodyIndex = i;
                            cache.probeIndex = p;
                            cache.positionWorld = pos;
                            cache.smallestDistance = dist;
                            cache.wallNormal = CalculateNormalWall(line, trans);
                        }
                    }
                    ++line;
                }
            }
        }
    }
    float wanderVolume = 1.0f;
    if(cache.bodyIndex != -1 )
    {
        float penDistance = (info->GetSourceActor()->GetPosition() + probes[cache.probeIndex] -
                             cache.positionWorld).length();
        btVector3 force = cache.wallNormal.normalize();
        force *= (penDistance * info->GetWallAvoidanceFactor());
        BehaviourI::FillArray(motion, &force, info->GetAvoidanceWeight());
        Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(), info->GetSourceActor()->GetPosition()+force, btVector3(1,1,0));
        Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(), cache.positionWorld, btVector3(1,0,0));
        wanderVolume = 1.0f - info->GetAvoidanceWeight(); 
    }

    ////Seek to the new0 location 
    if(m_wander == NULL) 
        m_wander  = new AvoidanceBehaviour();
    m_wander->CalcMotion(motion, info, wanderVolume);
}